﻿using System;
using System.ComponentModel;
using System.IO;
using System.Text;
using System.Xml.Serialization;
using Newtonsoft.Json;
using BaseDll;
using System.Net;
using System.Net.Sockets;
using System.Diagnostics;
using System.Windows.Forms;
using System.Threading;
using System.Collections.Generic;
using CommonTools;
using System.Reflection;
namespace MotionIoLib
{
    public class EpsonScaraRobotParam
    {
        public TcpParameter _cmdTcpParam = null;
        public TcpParameter _stateTcpParam = null;

        /// <summary>
        /// 启动机器人程序
        /// </summary>
        public string str_IoStartRobotProgram = "";
        /// <summary>
        /// 清除机器错误
        /// </summary>
        public string str_IoClearRobotErrer= "";
        /// <summary>
        /// 停止机器人程序
        /// </summary>
        public string str_IoStopRobotProgram = "";
        public int n_TimeofRestart= 1000;
        public int n_TimeofStop = 1000;
    }

    public class Robot_Epson:BaseScaraRobot
    {
        private EpsonScaraRobotParam RobotParam = new EpsonScaraRobotParam();
        private RobotTcpClient _cmdClient;
        private RobotTcpClient _statesClient;
        private Dictionary<int, bool> dic_IoInputState = new Dictionary<int, bool>();
        private Dictionary<int, bool> dic_IoOutputState = new Dictionary<int, bool>();
        private Coordinate currentPosition = new Coordinate() { X = -9999, Y = -9999, Z = -9999, U = -9999,Direction=HandDirection.None };
        private bool b_InPos = false;
        private bool b_ServoOn = false;
        private bool b_HighPower = false;
        private object lockObj = new object();
        Thread thread = null;
        private bool b_Exit = false;
        bool b_Reflush = false;
        public Robot_Epson(string strName, params object[] RobotParam):base(strName,RobotParam)
        {
            this.strName = strName;
            this.RobotParam._cmdTcpParam = new TcpParameter(RobotParam[0].ToString(), Convert.ToInt32(RobotParam[1].ToString()));
            this.RobotParam._stateTcpParam = new TcpParameter(RobotParam[2].ToString(), Convert.ToInt32(RobotParam[3].ToString()));
            this.RobotParam.str_IoStartRobotProgram = RobotParam[4].ToString();
            this.RobotParam.str_IoClearRobotErrer = RobotParam[5].ToString();
            this.RobotParam.str_IoStopRobotProgram = RobotParam[6].ToString();
        }
        bool b_LaseReflush = false;
        private void ThreadMonitor()
        {
            while (!b_Exit)
            {
                Thread.Sleep(50);
                if (!b_Reflush)
                    b_LaseReflush = true;
                else
                    b_LaseReflush = false;
                if (!IsConnect())
                    continue;
                string[] list_State = GetRobotAllState();
                if (list_State.Length < 10)
                    continue;
                foreach (var item_IoIn in dic_IoIn)
                {
                    int n_Index = item_IoIn.Value;
                    bool b_State = (UInt32.Parse(list_State[8]) & (1 << n_Index)) >0;
                    dic_IoInputState[n_Index] = b_State;
                }
                foreach (var item_IoOut in dic_IoOut)
                {
                    int n_Index = item_IoOut.Value;
                    bool b_State = (UInt32.Parse(list_State[7]) & (1 << n_Index)) > 0;
                    dic_IoOutputState[n_Index] = b_State;
                }
                currentPosition.Direction = list_State[5].ToDouble() <= 0 ? HandDirection.左手系 : HandDirection.右手系;
                currentPosition.X = list_State[1].ToDouble();//1
                currentPosition.Y = list_State[2].ToDouble();//2
                currentPosition.Z = list_State[3].ToDouble();//3
                currentPosition.U = list_State[4].ToDouble();//4
                b_ServoOn = list_State[9].ToByte() > 0 && list_State[10].ToByte() > 0;
                b_HighPower = list_State[10].ToByte() > 0 ? true : false;
                b_InPos = list_State[0] == "1";
                if(b_LaseReflush)
                   b_Reflush = true;
            }
        }

        public override bool Open()
        {
            try
            {
                if (IsConnect())
                    return false;
                #region robot client
                IOMgr.GetInstace().WriteIoBit(RobotParam.str_IoClearRobotErrer, true);
                Thread.Sleep(1000);
                IOMgr.GetInstace().WriteIoBit(RobotParam.str_IoClearRobotErrer, false);
                IOMgr.GetInstace().WriteIoBit(RobotParam.str_IoStartRobotProgram, false);
                Thread.Sleep(1000);
                IOMgr.GetInstace().WriteIoBit(RobotParam.str_IoStartRobotProgram, true);
                _cmdClient = new RobotTcpClient();
                _cmdClient.Init(RobotParam._cmdTcpParam.IpAddress, RobotParam._cmdTcpParam.Port, RobotParam._cmdTcpParam.Timeout);

                _statesClient = new RobotTcpClient();
                _statesClient.Init(RobotParam._stateTcpParam.IpAddress, RobotParam._stateTcpParam.Port, RobotParam._stateTcpParam.Timeout);
                Stopwatch stopwatch = new Stopwatch();
                stopwatch.Start();
                while (true)
                {
                    try
                    {
                        _cmdClient.Connect();
                        break;
                    }
                    catch (Exception ex)
                    {
                        Thread.Sleep(1000);
                        if (stopwatch.ElapsedMilliseconds > 10000)
                            throw ex;
                        continue;
                    }

                }
                string strRobotPos = _cmdClient.SendCommand($"003x@0");
                string[] arraypos = strRobotPos.Split(',');
                if (arraypos.Length < 4)
                    return false;
                _statesClient.Connect();
                string strRobotAgl = _cmdClient.SendCommand($"005x@0");
                var arrayagl = strRobotAgl.Split(',');
                if (arrayagl.Length < 4)
                    return false;
                #endregion
                foreach (var item in dic_IoIn)
                {
                    dic_IoInputState.Add(item.Value, false);
                }
                foreach (var item in dic_IoOut)
                {
                    dic_IoOutputState.Add(item.Value, false);
                }
                PowerOn(false);
                thread = new Thread(ThreadMonitor);
                thread.IsBackground = true;
                thread.Start();
                return true;
            }
            catch (Exception ex)
            {
                MessageBox.Show($"加载机器人 出现异常：{ex.Message},异常代码：{ex.StackTrace}");
                return false;

            }

        }

        public override bool Close()
        {
            dic_IoIn.Clear();
            dic_IoOut.Clear();
            if (_statesClient != null)
                _statesClient.bExit = true;
            if (_cmdClient != null)
                _cmdClient.bExit = true;
            if (_statesClient != null && _statesClient.bIsConnected)
            {
                _statesClient?.SendCommand("Exit");
                _statesClient?.Close();
            }
            if (_cmdClient != null && _cmdClient.bIsConnected)
            {
                _cmdClient?.SendCommand($"100Exit");
                _cmdClient?.Close();
            }
            b_Exit = true;

            Thread.Sleep(500);
            thread.Join(500);
            try
            {
                if (thread.IsAlive)
                    thread.Abort();
            }
            catch (Exception )
            {

            }

            return true;
        }

        private string[] GetRobotAllState()
        {
            lock (lockObj)
            {
                if (!(bool)(_statesClient?.Client?.Connected))
                    return null;
                var response = _statesClient.SendCommand("GetStates");
                var array = response.Split(',');
                return array;
            }
        }
        public override bool InPos(Coordinate coord)
        {
            bool b_CoordInpos = true;
            b_CoordInpos &= Math.Abs(currentPosition.X - coord.X) < 1;
            b_CoordInpos &= Math.Abs(currentPosition.Y - coord.Y) < 1;
            b_CoordInpos &= Math.Abs(currentPosition.Z - coord.Z) < 1;
            b_CoordInpos &= Math.Abs(currentPosition.U - coord.U) < 1;
            return b_InPos &&b_Reflush&& b_CoordInpos;
        }
        public override bool IsConnect()
        {
            if (!(_cmdClient != null && _cmdClient.bIsConnected))
                return false;
            if (!(_statesClient != null && _statesClient.bIsConnected))
                return false;
            return true;
        }

        public override bool IsServoOn()
        {
            return b_ServoOn;

        }
        public override bool IsPowerOn()
        {
            return b_HighPower;
        }
        public override bool GetInBit(int index)
        {
            if (dic_IoInputState.ContainsKey(index))
                return dic_IoInputState[index];
            else
                return false;

        }
        public override bool GetOutBit(int index)
        {
            if (dic_IoOutputState.ContainsKey(index))
                return dic_IoOutputState[index];
            else
                return false;
        }
        public override Coordinate GetCurrentCoordinate()
        {
            return currentPosition;
        }
        public override HandDirection GetCurrentHandDirection()
        {
            return currentPosition.Direction;
        }
        public override void ServoOn(bool bServo)
        {
            if (bServo)
                _cmdClient?.SendCommand("100motor on");
            else
                _cmdClient?.SendCommand("100motor off");

        }
        public override void PowerOn(bool bHighPower)
        {
            if (bHighPower)
                _cmdClient?.SendCommand("100power high");
            else
                _cmdClient?.SendCommand("100power low");
        }
        public override void ResetRobot()
        {
            _cmdClient.SendCommand("100Reset");
            if (IsConnect())
                return;
            IOMgr.GetInstace().WriteIoBit(RobotParam.str_IoClearRobotErrer, true);
            Thread.Sleep(1000);
            IOMgr.GetInstace().WriteIoBit(RobotParam.str_IoClearRobotErrer, false);
            IOMgr.GetInstace().WriteIoBit(RobotParam.str_IoStartRobotProgram, false);
            Thread.Sleep(1000);
            IOMgr.GetInstace().WriteIoBit(RobotParam.str_IoStartRobotProgram, true);
            _cmdClient = new RobotTcpClient();
            _cmdClient.Init(RobotParam._cmdTcpParam.IpAddress, RobotParam._cmdTcpParam.Port, RobotParam._cmdTcpParam.Timeout);

            _statesClient = new RobotTcpClient();
            _statesClient.Init(RobotParam._stateTcpParam.IpAddress, RobotParam._stateTcpParam.Port, RobotParam._stateTcpParam.Timeout);

        }
        public override void SetVel(double d_Vel)
        {
            if (d_Vel > 100 || d_Vel < 0)
            {
                return;
            }
            _cmdClient.SendCommand($"100Speed {d_Vel}");
        }
        public override void SetAcc(double d_Acc)
        {
            if (d_Acc > 120 || d_Acc < 0)
            {
                return;
            }
            _cmdClient.SendCommand($"100Accel {d_Acc},{d_Acc}");
        }
        public override bool Jump(Coordinate coordinate,  double limz, bool bCheckHandleSys = false)
        {
            StringBuilder sb = new StringBuilder();
            Coordinate CurrentPosition = GetCurrentCoordinate();
            limz = limz > (CurrentPosition.Z > coordinate.Z ? CurrentPosition.Z : coordinate.Z) ?
                   limz : (CurrentPosition.Z > coordinate.Z ? CurrentPosition.Z : coordinate.Z) + 0.02;
            sb.Append($"100Till MemSw(1) = On;jump XY({coordinate.X},{coordinate.Y},{coordinate.Z},{coordinate.U})");
            if (coordinate.Direction != HandDirection.None)
            {
                if (coordinate.Direction == HandDirection.左手系)
                    sb.Append($" /L");
                else if (coordinate.Direction == HandDirection.右手系)
                    sb.Append($" /R");
                else return false;
            }
            if (limz != 0)
                sb.Append($" LimZ {limz}");
            sb.Append(" Till;MemOff 1");
            BaseDll.DeviceDataCollect.m_DeviceData.Function_MachineLog(strName, sb.ToString());
            var result = _cmdClient.SendCommand(sb.ToString());
            if (result != "0")
            {
                Stop();
                return false;
            }
            return true;

        }
        public override bool JumpInpos(Coordinate coordinate, bool bmanual, double limz, bool bCheckHandleSys = false, int n_TimeOut = 30000)
        {
            StationCheck(bmanual);
            Jump(coordinate, limz, bCheckHandleSys);
            Thread.Sleep(200);
            Stopwatch stopwatch = new Stopwatch();
            stopwatch.Restart();
            // while (!ts.IsCompleted)
            while (!InPos(coordinate))
            {
                if (stopwatch.ElapsedMilliseconds > n_TimeOut)
                {
                    throw new Exception($"机械人{n_TimeOut}毫秒超时" + "运动失败");
                }

            }
            return true;
        }
        public override bool Go(Coordinate coordinate, bool bCheckHandleSys = false)
        {
            if (bCheckHandleSys)
            {
                if (!CheckRLSystem(coordinate.Direction))
                {
                    return false;
                }
            }
            bool bSave = true;
            bSave = RobotMotionMgr.GetInstace().RobotMoveSafeCheck(GetCurrentCoordinate(), coordinate);
            if (!bSave)
                throw new Exception("机器人运动指令没有通过安全监测");

            ResetStopActionFlag();
            StringBuilder sb = new StringBuilder();
            string command = $"100Till MemSw(1)= On;GO XY({coordinate.X},{coordinate.Y},{coordinate.Z},{coordinate.U})";
            sb.Append(command);
            BaseDll.DeviceDataCollect.m_DeviceData.Function_MachineLog(strName, command);

            if (coordinate.Direction != HandDirection.None)
            {
                if (coordinate.Direction == HandDirection.左手系)
                    sb.Append($" /L");
                else if (coordinate.Direction == HandDirection.右手系)
                    sb.Append($" /R");
                else return false;
            }
            sb.Append(" Till;MemOff 1");
            var result = _cmdClient?.SendCommand(sb.ToString());
            return true;

        }
        public override bool GoInpos(Coordinate coordinate, bool bmanual, bool bCheckHandleSys = false, int n_TimeOut = 30000)
        {
            StationCheck(bmanual);
            Go(coordinate, bCheckHandleSys);
            b_Reflush = false;
            Stopwatch stopwatch = new Stopwatch();
            stopwatch.Restart();
            while (true)
            {
                if (InPos(coordinate))
                {
                    ResetStopActionFlag();
                    return true;
                }
                if (stopwatch.ElapsedMilliseconds > n_TimeOut)
                {
                    ResetStopActionFlag();
                    string[] list_Input = new string[] { coordinate.X.ToString(),
                                                         coordinate.Y.ToString(),
                                                         coordinate.Z.ToString(),
                                                         coordinate.U.ToString(),
                                                         coordinate.Direction.ToString()};
                    AlarmMgr.GetIntance().WarnWithDlg((int)ScaraAlarm.四轴机器人运动到位超时, new string[] { "退出" }, list_Input, "Epson四轴机器人运动到位失败");
                    return false;
                }
                Thread.Sleep(10);
            }
        }
        public override bool Move(Coordinate coordinate, bool bCheckHandleSys)
        {
            if (bCheckHandleSys)
            {
                if (!CheckRLSystem(coordinate.Direction))
                {
                    return false;
                }
            }
            StringBuilder sb = new StringBuilder();
            if (coordinate.U >= 360)
                coordinate.U = coordinate.U - 360;
            if (coordinate.U <= -360)
                coordinate.U = coordinate.U + 360;
            string command = $"100Till MemSw(1) = On;Move XY({coordinate.X},{coordinate.Y},{coordinate.Z},{coordinate.U})";
            sb.Append(command);
            BaseDll.DeviceDataCollect.m_DeviceData.Function_MachineLog(strName, command);
            if (coordinate.Direction != HandDirection.None)
            {
                if (coordinate.Direction == HandDirection.左手系)
                    sb.Append($" /L");
                else if (coordinate.Direction == HandDirection.右手系)
                    sb.Append($" /R");
                else return false;
            }
            sb.Append(" Till;MemOff 1");
            var result = _cmdClient.SendCommand(sb.ToString());
            if (result != "0")
            {

                Stop();
                return false;
            }
            return true;

        }
        public override bool MoveInpos(Coordinate coordinate,bool bmanual, bool bCheckHandleSys, int n_TimeOut = 30000)
        {
            StationCheck(bmanual);
            Move(coordinate, bCheckHandleSys);
            Stopwatch stopwatch = new Stopwatch();
            stopwatch.Restart();
            // while (!ts.IsCompleted)
            while (!InPos(coordinate))
            {
                if (stopwatch.ElapsedMilliseconds > n_TimeOut)
                {
                    throw new Exception($"机械人{n_TimeOut}毫秒超时" + "运动失败");
                }

            }
            return true;

        }
        private void StationCheck(bool bmanual)
        {
            if (GlobalVariable.g_StationState == StationState.StationStatePause)
            {
                while (true)
                {
                    Thread.Sleep(10);
                    if (GlobalVariable.g_StationState == StationState.StationStateRun)
                        break;
                    else if (GlobalVariable.g_StationState == StationState.StationStatePause)
                        continue;
                    else
                        throw new Exception("自动运行停止");
                }
            }
            if (GlobalVariable.g_StationState == StationState.StationStateRun && bmanual)
            {
                throw new Exception("自动运行的情况下，禁止bmanual=true的运动指令");
            }
            if (GlobalVariable.g_StationState != StationState.StationStateRun && !bmanual)
            {
                throw new Exception("手动运行的情况下，禁止bmanual=false的运动指令");
            }

        }
        public override bool CheckIoByName( string strIoName, bool b_State, int n_TimeOut = 3000,int n_FilterTime=20)
        {
            Retry:
            Stopwatch stopwatch = new Stopwatch();
            stopwatch.Restart();
            if (!dic_IoIn.ContainsKey(strIoName))
                return false;
            while (true)
            {
                if (Function_ReCheckIO(strIoName, b_State, n_FilterTime))
                    return true;
                if (stopwatch.ElapsedMilliseconds>n_TimeOut)
                {
                    string[] list_Control = GlobalVariable.g_StationState!=StationState.StationStateRun ? new string[] { "重试", "退出" } : new string[] { "重试" };                    
                    string[] list_Input = new string[] { strIoName, b_State.ToString(), dic_IoIn[strIoName].ToString() };
                    WaranResult result = AlarmMgr.GetIntance().WarnWithDlg((int)ScaraAlarm.四轴机器人气缸到位失败, list_Control, list_Input,"四轴机器人气缸等待超时", null);
                    if (result == WaranResult.Custom1)
                        goto Retry;
                    else
                        return false;
                }
            }
            
        }
        private bool Function_ReCheckIO(string ioName, bool isOn, int nFilterTime = 50)
        {
            Stopwatch watch_ReCheck = new Stopwatch();
            while (true)
            {
                if (GetInBit(dic_IoIn[ioName]) != isOn)
                    return false;
                else
                {
                    if (!watch_ReCheck.IsRunning)
                         watch_ReCheck.Start();
                }
                if (watch_ReCheck.ElapsedMilliseconds > nFilterTime)
                {
                    return true;
                }                    
                else
                    Thread.Sleep(1);
            }
        }

        public override void Stop()
        {
            _cmdClient?.SendCommand("100MemOff 1");
        }
        public override bool SetStopActionFlag()
        {
            var result = _statesClient?.SendCommand("100MemOn 1", true);
            if (result != "0")
                return false;
            return true;
        }
        public override bool ResetStopActionFlag()
        {
            var result = _statesClient?.SendCommand("100MemOff 1", true);
            if (result != "0")
                return false;
            return true;

        }
        public override bool Home()
        {
            var result = _cmdClient?.SendCommand("100Home");
            if (result != "0")
            {
                _cmdClient?.SendCommand("100MemOff 1");

                return false;
            }
            return true;

        }
        public override void SetOutput(string strIoName, bool value)
        {
            int index = 0;
            if (!dic_IoOut.ContainsKey(strIoName))
                return;
            else
                index = dic_IoOut[strIoName];
            var cmd = value ? "On" : "Off";
            var result = _cmdClient.SendCommand($"100{ cmd }({index})");

        }
        public override bool ReadOutput(string strIoName)
        {
            int index = 0;
            if (!dic_IoOut.ContainsKey(strIoName))
                return false;
            else
                index = dic_IoOut[strIoName];
            return dic_IoOutputState[index];
        }
        public override bool ReadInput(string strIoName)
        {
            int index = 0;
            if (!dic_IoIn.ContainsKey(strIoName))
                return false;
            else
                index= dic_IoIn[strIoName];
            return dic_IoInputState[index];
        }
        public override bool CheckRLSystem(HandDirection handDirection)
        {
            return true;
        }

        public override bool RepeatLink()
        {
            return Open();

        }

    }
}